//////////////////////////////////
//
// for camera frame transform to gps frame
//
/////////////////////////////////
#include "vision_ruler/GlobalUtil.h"

vision_ruler::GlobalUtil::GlobalUtil() {}
vision_ruler::GlobalUtil::~GlobalUtil() {}

void vision_ruler::GlobalUtil::computePose(std::vector<vision_ruler::box_info>& boxes, vision_ruler::global_info ginfo) {
    if(boxes.size() > 0) {
        double latitude = ginfo.latitude;
        double longitude = ginfo.longitude;
        double altitude = ginfo.altitude;
        double pose_yaw = ginfo.pose_yaw * PI / 180.0;
        double pose_pitch = ginfo.pose_pitch * PI / 180.0;
        bool withGps = ginfo.withGps;
        double xyz[3];
        if(withGps) {
            geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);
            simple_logger::getInstance()->debug("gps direction is %.2f degree", ginfo.pose_yaw);
        }
        for(size_t i = 0; i < boxes.size(); i++) {
            double yaw = boxes[i].yaw;
            double gpsXYZ[3];
            int comefrom=boxes[i].comefrom;
            if(comefrom==0){
                double camera_gps_degree=ginfo.camera_gps_degree * PI / 180.0;
                boxes[i].distance = boxes[i].distance * std::cos(pose_pitch);
                simple_logger::getInstance()->debug("box direction is %.2f degree, distance is %.2f", yaw * 180.0 / PI, boxes[i].distance);
                if(withGps) {
                    geoConverter.Reverse(xyz[0]+boxes[i].distance*std::sin(pose_yaw+yaw+camera_gps_degree)*1.0081, xyz[1]+boxes[i].distance*std::cos(pose_yaw+yaw+camera_gps_degree)*1.55165, xyz[2], gpsXYZ[0], gpsXYZ[1], gpsXYZ[2]);
                    boxes[i].latitude = gpsXYZ[0];
                    boxes[i].longitude = gpsXYZ[1];
                    boxes[i].altitude = gpsXYZ[2];
                } else {
                    boxes[i].latitude = 0;
                    boxes[i].longitude = 0;
                    boxes[i].altitude = 0;
                }
            } else if(comefrom=1) {
                double lidar_gps_degree=ginfo.lidar_gps_degree * PI / 180.0;
                if(withGps) {
                    geoConverter.Reverse(xyz[0]+boxes[i].lidar_estimate_distance*std::sin(pose_yaw+lidar_gps_degree)*1.0081, xyz[1]+boxes[i].lidar_estimate_distance*std::cos(pose_yaw+lidar_gps_degree)*1.55165, xyz[2], gpsXYZ[0], gpsXYZ[1], gpsXYZ[2]);
                    boxes[i].latitude = gpsXYZ[0];
                    boxes[i].longitude = gpsXYZ[1];
                    boxes[i].altitude = gpsXYZ[2];
                } else {
                    boxes[i].latitude = 0;
                    boxes[i].longitude = 0;
                    boxes[i].altitude = 0;
                }
            }
            
        }
    }
}
